 #include <cv.h>  
 #include <highgui.h>  
 #include <stdio.h>  
 #include <stdlib.h>  
 
 using namespace std;
 using namespace cv;
 
  
 // ----- HOMOGRAPHY FUNCTION --------------------------------------
 int getHomography(int board_w, int board_h, char * lfilename, char * rfilename, bool print)
 {
	 int board_n  = board_w * board_h;  
	 CvSize board_sz = cvSize( board_w, board_h );
	 
	 
	 // Load left and right images
	 
	 for (int i = 3;i<4;i++){
		 IplImage *image = 0, *gray_image = 0;  
		 if((image = cvLoadImage(lfilename))== 0){
			 printf("Error: Couldn't load %s\n",lfilename);
			 return -1;  
		  }  
 	  gray_image = cvCreateImage(cvGetSize(image),IPL_DEPTH_8U,1);  
 	  cvCvtColor(image, gray_image, CV_BGR2GRAY);  

	// Detect checkboarb corners on the image
 	CvPoint2D32f* corners = new CvPoint2D32f[ board_n ];  
 	int corner_count = 0;  
 	int found = cvFindChessboardCorners(  
 		image,  
 		board_sz,  
 		corners,  
 		&corner_count,   
 		CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS  
 	  );  
 	if(!found){  
 		printf("Couldn't aquire checkerboard");  
 		return -1;  
 	}  
 	// Get Subpixel accuracy on those corners  
 	cvFindCornerSubPix(gray_image, corners, corner_count,   
 			  cvSize(11,11),cvSize(-1,-1),   
 			  cvTermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));  
 	
 	//GET THE IMAGE AND OBJECT POINTS:  
 	//Object points are at (r,c): (0,0), (board_w-1,0), (0,board_h-1), (board_w-1,board_h-1)  
 	//That means corners are at: corners[r*board_w + c]  
 	CvPoint2D32f objPts[4], imgPts[4];  
 	objPts[0].x = 0;         objPts[0].y = 0;   
 	objPts[1].x = board_w-1; objPts[1].y = 0;   
 	objPts[2].x = 0;         objPts[2].y = board_h-1;  
 	objPts[3].x = board_w-1; objPts[3].y = board_h-1;   
 	imgPts[0] = corners[0];  
 	imgPts[1] = corners[board_w-1];  
 	imgPts[2] = corners[(board_h-1)*board_w];  
 	imgPts[3] = corners[(board_h-1)*board_w + board_w-1];  
 	
 	// Print results 
 	if (print) {
	 	
	 	cvNamedWindow("Checkers"); 	
	 	//Draw the points in order: B,G,R,YELLOW  
	 	cvCircle(image,cvPointFrom32f(imgPts[0]),9,CV_RGB(0,0,255),3);  
	 	cvCircle(image,cvPointFrom32f(imgPts[1]),9,CV_RGB(0,255,0),3);  
	 	cvCircle(image,cvPointFrom32f(imgPts[2]),9,CV_RGB(255,0,0),3);  
	 	cvCircle(image,cvPointFrom32f(imgPts[3]),9,CV_RGB(255,255,0),3);  
	 	//Draw the found checkboard  
	 	cvDrawChessboardCorners(image, board_sz, corners, corner_count, found);  
	 	cvShowImage( "Checkers", image );  
	}
	
	//FIND THE HOMOGRAPHY  
	CvMat *H = cvCreateMat( 3, 3, CV_32F);  
	CvMat *H_invt = cvCreateMat(3,3,CV_32F);  
	cvGetPerspectiveTransform(objPts,imgPts,H);  
	
	
	//LET THE USER ADJUST THE Z HEIGHT OF THE VIEW  
	float Z = 25;  
	int key = 0;  
	IplImage *birds_image = cvCloneImage(image);  
	cvNamedWindow("Birds_Eye");
	cvNamedWindow("Birds_Eye2"); 
	IplImage *image2 = 0, *gray_image2 = 0, *gray_image3 = 0;  
	
	
	image2 = cvLoadImage(rfilename);
	gray_image2 = cvCreateImage(cvGetSize(image2),8,1);  
	cvCvtColor(image2, gray_image2, CV_BGR2GRAY); 
	IplImage *birds_image2 = cvCloneImage(image2);  
	gray_image3 = cvCreateImage(cvGetSize(birds_image2),8,1);  


//while(key != 27) {//escape key stops  
	   CV_MAT_ELEM(*H,float,2,2) = 25;  
		cvWarpPerspective(image,birds_image,H,  
					CV_INTER_LINEAR+CV_WARP_INVERSE_MAP+CV_WARP_FILL_OUTLIERS );  
	   cvShowImage("Birds_Eye", birds_image);  		

		Mat hMat(H);
		
		return 0;
		// and has been allocated and cv::Mat Mimg has been defined
	/*	Mat hMat(H);  //Construct an Mat image "img" out of an IplImage
		Mat srcMat = (Mat_<float>(3, 1) << imgPts[0].x, imgPts[0].y, 1);
		Mat inverse = hMat.inv();
		Mat dstMat = inverse*srcMat;

		CvPoint2D32f temp;
		temp.x = dstMat.at<float>(0,0)/dstMat.at<float>(2,0);
		temp.y = dstMat.at<float>(1,0)/dstMat.at<float>(2,0);
		CvPoint2D32f temp2;

		Mat srcMat2 = (Mat_<float>(3, 1) << 1900+50+80+150, 1080+50+80, 1);
		Mat dstMat2 = inverse*srcMat2;
		temp2.x = 1900+50+80+150;//dstMat2.at<float>(0,0)/dstMat2.at<float>(2,0);
		temp2.y = 1080+50+80;//dstMat2.at<float>(1,0)/dstMat2.at<float>(2,0);

		cout << "\nDetected homography matrix : " << hMat;
		cout << "\nOriginal points : " << imgPts[0].x << "," << imgPts[0].y << "\n";
		cout << "\nDetected corners : " << temp.x << "," << temp.y << "\n";*/

//	}  

}
}


 // ----- HOMOGRAPHY FUNCTION --------------------------------------
 
 
 int main(int argc, char* argv[]) {  
 	
 	String s = "photos/r01.jpg";
	char *fileName = (char*)s.c_str();
 	getHomography(9,6,fileName,fileName,true);
 	return 1;  
 }  
 
